package com.rockwell.sniffyhunter.model;

import java.util.ArrayDeque;
import java.util.Deque;

import com.rockwell.sniffyhunter.utils.IRobotCommander;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.RobotCommandSequencerState;
import com.rockwell.sniffyhunter.utils.TurnerState;

public class RobotCommandSequencer extends StateObject {

	private static final String TAG = "RobotCommandSequencer";
	
	public static int INTERVAL = 50;
	public static int CMD_TIME_OUT = 30000; // Milliseconds
	public static int PICK_TIME_OUT = 120000; // Milliseconds
	public static int MOTION_SETTLING_TIME = 0; // Milliseconds
	public static int MOTION_SETTLING_TIME2 = 3000; // Milliseconds
	public static int COMPASS_SETTLING_TIME = 1200; // Milliseconds
	private static final int TURN_TOLERANCE = 1;
	private static final int ROUGH_TURN_TOLERANCE = 10;
	private static final int MINIMUM_TURN_THRESHOLD = 2;
	private boolean m_useRoughTolerance = false;
	private boolean m_useCompass = true;
	
	private int m_cmdTime = 0;
	private int m_cmdTimeOut = 0;
	private IRobotSensors m_sensors;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private IRobotCommander m_commander;
	private RobotCommand m_curCommand;
	private Deque<RobotCommand> m_commands;
	private int m_timeout = 0;
	private double m_encoder = 0;
	private static int m_closeReverseAngle = 25;
	private static int m_closeReverseDistance = 20;
	private static int m_tooCloseReverseAngle = 20;
	private static int m_tooCloseReverseDistance = 30;
	private static final int m_overtravelAnglePlus = 0;//10;
	private static final int m_overtravelAngleMinus = 0;//-20;
	
	private ExitCode m_exitCode;
	
	private enum ExitCode {
		UNINITIALIZED,
		Error,
	}
	
	private boolean isError() {
		return m_exitCode == ExitCode.Error;
	}


	public RobotCommandSequencer(Robot robot) {
		super(robot);
		m_sensors = robot.m_sensors;
		m_positionDetector = robot.m_positionDetector;
		m_commander = robot.m_commander;
		m_curCommand = new RobotCommand();
		m_commands = new ArrayDeque<RobotCommand>(4);
		m_useRoughTolerance = false;
	}
	
	public void setUseRoughTolerance(boolean value) {
		m_useRoughTolerance = value;
	}
	
	public void setUseCompass(boolean value) {
		m_useCompass = value;
	}

	@Override
	protected void Initialize() {
		machineState = RobotCommandSequencerState.START;
	}

	@Override
	protected boolean IsDone() {
		return machineState == RobotCommandSequencerState.DONE;
	}
	
	public ReturnResult PerformAction(RobotAction robotAction)  {
		
		if (machineState == RobotCommandSequencerState.START)
			setRobotAction(robotAction, 0, CMD_TIME_OUT);

		return PerformAction();
	}
	
	public ReturnResult  PerformAction(RobotAction robotAction, double param)  {
		if (machineState == RobotCommandSequencerState.START)
			setRobotAction(robotAction, param, CMD_TIME_OUT);

		return PerformAction();
	}
	
	public ReturnResult PerformActionPick(RobotAction robotAction)  {
		if (machineState == RobotCommandSequencerState.START)
			setRobotAction(robotAction, 0, PICK_TIME_OUT);

		return PerformAction();
	}

	private void setRobotAction(RobotAction robotAction, double param, int timeout) {
		m_encoder = 0;
		m_cmdTime = 0;
		m_cmdTimeOut = timeout;
		m_exitCode = ExitCode.UNINITIALIZED;
		
		if (robotAction == RobotAction.TurnBack ||
			(robotAction == RobotAction.TurnDegree && (param == IRobotCommander._180_DEGREE || param == -IRobotCommander._180_DEGREE))) {
			double leftWallDistance = m_sensors.getLeftDistance();
			double rightWallDistance = m_sensors.getRightDistance();
			
			robotAction = RobotAction.TurnDegree;
			
			if (leftWallDistance > rightWallDistance) {
				param = -IRobotCommander._180_DEGREE;
			}
			else {
				param = IRobotCommander._180_DEGREE;
			}
		}
		
		m_curCommand.param1 = param;
		m_curCommand.action = robotAction;
		
		setTargetOrientation();
		expandCommands();
	}
	
	public IStateMachine performSendCommand() {
		if (!sendCommand()) {
			machineState = RobotCommandSequencerState.ERROR;
			return machineState;
		}

		String msg = String.format("Send command %s[%.1f] done.", m_curCommand.action.name(), m_curCommand.param1);
		log(msg, 4);
		
		if (m_curCommand.action == RobotAction.Jog) {	
			machineState = RobotCommandSequencerState.DONE;	
		}
		else {
			machineState = RobotCommandSequencerState.WAIT;
		}
		return machineState;
	}
	
	public IStateMachine performWaitCommandDone() {
		
		int status = peekCommandDone();
		
		if (status == 1) {
			boolean redo = false;
			
			if (m_useCompass && robot.isUseCompassCompensationDuringTurn() && isTurning()) {
				sleep(COMPASS_SETTLING_TIME);
				double currentOrientation = m_sensors.getOrientation();
				double orientationDif = m_curCommand.targetOrientation - currentOrientation;
				
				if (orientationDif > 180)
					orientationDif = orientationDif - 360;
				else if (orientationDif < -180)
					orientationDif = orientationDif + 360;

				boolean useRoughTolerance = (m_useRoughTolerance || 
											(m_curCommand.action == RobotAction.TurnDegree && Math.abs(m_curCommand.param1) <= 45));
				
				if (Math.abs(orientationDif) > (useRoughTolerance? ROUGH_TURN_TOLERANCE : TURN_TOLERANCE)) {
					if (Math.abs(orientationDif) < MINIMUM_TURN_THRESHOLD) {
						m_curCommand.action = RobotAction.TurnDegree;
						m_curCommand.param1 = orientationDif > 0? MINIMUM_TURN_THRESHOLD + 2 : -MINIMUM_TURN_THRESHOLD - 2;
					}
					else {
						m_curCommand.action = RobotAction.TurnDegree;
						m_curCommand.param1 = orientationDif;
					}
					
					redo = true;
					machineState = RobotCommandSequencerState.SEND;

					String msg = String.format("Wait command %s done. Turn again[%.1f, %.1f]", m_curCommand.action.name(), m_curCommand.param1, currentOrientation);
					log(msg, 4);
				}
			}
			
			if (!redo) {
				double currentOrientation = m_sensors.getOrientation();
				String msg = String.format("Wait command %s done. Current angle [%.1f]", m_curCommand.action.name(), currentOrientation);
				log(msg, 4);
				
				if (m_commands.isEmpty()) {
					machineState = RobotCommandSequencerState.DONE;
				}
				else {
					m_cmdTime = 0;
					m_curCommand = m_commands.pop();
					machineState = RobotCommandSequencerState.SEND;
				}
			}
		}
		else if (status == -1) {
			machineState = RobotCommandSequencerState.ERROR;
		}
		
		//m_encoder = m_sensors.getActiveEncoderValue();
		
		return machineState;
	}
	
	public double getEncoderValue() {
		return m_encoder;
	}
	
	public IStateMachine performHandleError() {
		reconnectModbus();
		
		sleep(INTERVAL);
		
		String msg = String.format("Send Command[%s] Failed @ performSendCommand", m_curCommand.action.name());
		log(msg, 0);
		
		m_exitCode = ExitCode.Error;
		machineState = RobotCommandSequencerState.DONE;
		
		return machineState;
	}

	private boolean sendCommand() {
		try {
			m_commander.sendCommand(m_curCommand);
			return true;
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			return false;
		}
	}
	
	private void reconnectModbus() {
		try {
			m_commander.disconnect();
			m_commander.connect();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			log(e, 1);
			e.printStackTrace();
		}
	}
	
	private int peekCommandDone() {
		try {
			if (m_commander.isCommandDone()) {
				return 1;
			}
			else if (m_cmdTime < m_cmdTimeOut){
				m_cmdTime += INTERVAL;
				sleep(INTERVAL);
				return 0;
			}
			else {
				return -1;
			}
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			
			return -1;
		}
	}
	
	private int peekJogDone() {
		try {
			if (m_commander.isJogDone()) {
				return 1;
			}
			else if (m_cmdTime < m_cmdTimeOut){
				m_cmdTime += INTERVAL;
				sleep(INTERVAL);
				return 0;
			}
			else {
				return -1;
			}
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			
			return -1;
		}
	}
	
	private boolean isTurning() {
		if (m_curCommand.action == RobotAction.TurnBack ||
			m_curCommand.action == RobotAction.TurnDegree ||
			m_curCommand.action == RobotAction.TurnLeft ||
			m_curCommand.action == RobotAction.TurnRight) {
			return true;
			}
		
		return false;
	}
	
	private void setTargetOrientation() {
		
		if (!isTurning())
			return;
		
		sleep(COMPASS_SETTLING_TIME);
		
		double targetOrientation = m_sensors.getOrientation();
		
		if (m_curCommand.action == RobotAction.TurnBack)
			targetOrientation += 180 + m_overtravelAngleMinus;
		else if (m_curCommand.action == RobotAction.TurnDegree)
			targetOrientation += m_curCommand.param1 + (m_curCommand.param1 > 0? m_overtravelAngleMinus: m_overtravelAnglePlus);
		else if (m_curCommand.action == RobotAction.TurnLeft)
			targetOrientation -= 90 + m_overtravelAnglePlus;
		else if (m_curCommand.action == RobotAction.TurnRight)
			targetOrientation += 90 - m_overtravelAngleMinus;
		
		m_curCommand.targetOrientation = targetOrientation;
		
		String msg = String.format("Target Orientation[%.1f]", targetOrientation);
		log(msg, 4);
	}
	
	public void expandCommands() {

		m_commands.clear();
		
		if (!isTurning())
			return;

		if (m_curCommand.param1 == 0 ||
			Math.abs(m_curCommand.param1) == 90 ||
			Math.abs(m_curCommand.param1) == 180) {
			
			m_robotPosition = m_positionDetector.getPosition();
			double curOrientation = m_sensors.getOrientation();
			
			double reverseDistance = 0;
			if (m_robotPosition.isFrontTooClose()) {
				reverseDistance = m_tooCloseReverseDistance;
			}
			else {
				reverseDistance = m_closeReverseDistance;
			}
			
			boolean expanded = false;
			
			if (m_robotPosition.isTooLeft()) {
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, -m_tooCloseReverseAngle, curOrientation - m_tooCloseReverseAngle));
				m_commands.add(new RobotCommand(RobotAction.MoveBackward, reverseDistance));
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, m_tooCloseReverseAngle, curOrientation + m_tooCloseReverseAngle));
				if (!m_robotPosition.isFrontCloseOrTooClose()) {
					m_commands.add(new RobotCommand(RobotAction.MoveForward2, reverseDistance));
				}
				expanded = true;
			}
			else if (m_robotPosition.isLeft()) {
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, -m_closeReverseAngle, curOrientation - m_closeReverseAngle));
				m_commands.add(new RobotCommand(RobotAction.MoveBackward, reverseDistance));
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, m_closeReverseAngle, curOrientation + m_closeReverseAngle));
				if (!m_robotPosition.isFrontCloseOrTooClose()) {
					m_commands.add(new RobotCommand(RobotAction.MoveForward2, reverseDistance));
				}
				expanded = true;
			}
			else if (m_robotPosition.isTooRight()) {
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, m_tooCloseReverseAngle, curOrientation + m_tooCloseReverseAngle));
				m_commands.add(new RobotCommand(RobotAction.MoveBackward, reverseDistance));
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, -m_tooCloseReverseAngle, curOrientation - m_tooCloseReverseAngle));
				if (!m_robotPosition.isFrontCloseOrTooClose()) {
					m_commands.add(new RobotCommand(RobotAction.MoveForward2, reverseDistance));
				}
				expanded = true;
			}
			else if (m_robotPosition.isRight()) {
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, m_closeReverseAngle, curOrientation + m_closeReverseAngle));
				m_commands.add(new RobotCommand(RobotAction.MoveBackward, reverseDistance));
				m_commands.add(new RobotCommand(RobotAction.TurnDegree, -m_closeReverseAngle, curOrientation - m_closeReverseAngle));
				if (!m_robotPosition.isFrontCloseOrTooClose()) {
					m_commands.add(new RobotCommand(RobotAction.MoveForward, reverseDistance));
				}
				expanded = true;
			}
			else if (m_robotPosition.isFrontTooClose()) {
				m_commands.add(new RobotCommand(RobotAction.MoveBackward2, m_tooCloseReverseDistance));
				expanded = true;
			}
			else if (m_robotPosition.isFrontClose()) {
				m_commands.add(new RobotCommand(RobotAction.MoveBackward2, m_closeReverseDistance));
				expanded = true;
			}
			
			if (expanded) {
				m_commands.add(m_curCommand);
				m_curCommand = m_commands.pop();
			}
		}
	}
}
